﻿
using UnityEditor; 
using UnityEngine;
using XFGameFramework;
using XFGameFramework.OctreeSystem; 

public class OctreeCylinderBounds : OctreeBounds
{

    #region 字段

    [SerializeField]
    [Tooltip("高度")]
    private float height = 1;
    [SerializeField]
    [Tooltip("半径")]
    private float radius = 0.5f;
    [SerializeField]
    [Tooltip("中心")]
    private Vector3 center;
      
    private Vector3 p;

    private OctreePlane plane_up = new OctreePlane();
    private OctreePlane plane_down = new OctreePlane();

    private OctreePlane plane = new OctreePlane();
     
    [Tooltip("从下往上")]
    private Ray ray = new Ray();
     
    [Tooltip("从上往下")]
    private Ray ray_inverse = new Ray();


    [Tooltip("世界坐标中的边界!")]
    private Bounds world_bounds;

    #endregion


    #region 属性

    public float Height
    {
        get
        {
            return height;
        }
        set
        {
            height = value;
        }
    }

    public float Radius
    {
        get
        {
            return radius;
        }
        set
        {
            radius = value;
        }
    }

    public Vector3 Center
    {
        get
        {
            return center;
        }
        set
        {
            center = value;
        }
    }
     
    private OctreePlane PlaneUp
    {
        get
        {
            plane_up.SetCenterAndNormal(Center + Vector3.up * Height / 2, Vector3.up, radius * 2, radius * 2 ,Vector3.right,Vector3.forward );
            return plane_up;
        }
    }

    private OctreePlane PlaneDown
    {
        get
        {
            plane_down.SetCenterAndNormal(Center + Vector3.down * Height / 2, Vector3.down, radius * 2, radius * 2, Vector3.right, Vector3.forward);
            return plane_down;
        }
    }

    internal Ray Ray
    {
        get {
            ray.origin = Center - Vector3.up * Height / 2;
            ray.direction = Vector3.up; 
            return ray;
        }
    }

    internal Ray RayInverse
    {
        get
        { 
            ray_inverse.origin = Center + Vector3.up * Height / 2;
            ray_inverse.direction = Vector3.down;

            return ray_inverse;
        }
    }

    #endregion

#if UNITY_EDITOR

    private void OnDrawGizmos()
    {
        Handles.color = Color.green;

        Handles.matrix = transform.localToWorldMatrix;

        Handles.DrawWireDisc(Center - Vector3.up * height / 2, Vector3.up, radius);

        Vector3 bottom_forward = Center - Vector3.up * height / 2 + Vector3.forward * radius;
        Vector3 bottom_back = Center - Vector3.up * height / 2 + Vector3.back * radius;
        Vector3 bottom_left = Center - Vector3.up * height / 2 + Vector3.left * radius;
        Vector3 bottom_right = Center - Vector3.up * height / 2 + Vector3.right * radius;


        Handles.DrawWireDisc(Center + Vector3.up * height / 2, Vector3.up, radius);

        Vector3 top_forward = Center + Vector3.up * height / 2 + Vector3.forward * radius;
        Vector3 top_back = Center + Vector3.up * height / 2 + Vector3.back * radius;
        Vector3 top_left = Center + Vector3.up * height / 2 + Vector3.left * radius;
        Vector3 top_right = Center + Vector3.up * height / 2 + Vector3.right * radius;

        Handles.DrawLine(bottom_forward, top_forward);
        Handles.DrawLine(bottom_back, top_back);
        Handles.DrawLine(bottom_left, top_left);
        Handles.DrawLine(bottom_right, top_right);
    }

#endif

    #region 抽象方法

    public override Vector3 ClosestPoint(Vector3 point)
    {
        Vector3 local = transform.worldToLocalMatrix.MultiplyPoint(point);
        Vector3 c = Center;
        c.y = local.y;
        float distance = Vector3.Distance(local, c);
        Vector3 close = c + (local - c).normalized * (distance > radius ? radius : distance);
        close.y = Mathf.Clamp(close.y, Center.y - Height / 2, Center.y + Height / 2);
        return transform.localToWorldMatrix.MultiplyPoint(close);
    }

    public override bool Contains(Vector3 point)
    {
        Vector3 local = transform.worldToLocalMatrix.MultiplyPoint(point);
        if (local.y > Center.y + Height / 2 || local.y < Center.y - Height / 2) return false;

        Vector3 c = Center;
        c.y = local.y;
        float distance = Vector3.Distance(local, c);
        return distance <= radius;
    }

    public override bool IntersectRay(Ray ray, float distance = -1)
    {
        IntersectInfo info;
        return IntersectRay(ray, out info, distance);
    }

    public override bool IntersectRay(Ray ray, out IntersectInfo info, float distance = -1)
    {
        info = new IntersectInfo();
        info.bounds = this;
        // 判断射线的起点是否在圆柱内部
        if (Contains(ray.origin))
        {
            // 如果在内部一定相交
            info.point = ray.origin;
            return true;
        }
  
        Ray ray1 = new Ray();
        ray1.origin = transform.worldToLocalMatrix.MultiplyPoint(ray.origin);
        ray1.direction = transform.worldToLocalMatrix.MultiplyVector(ray.direction).normalized;

        if (distance != -1)
        {
            distance = Vector3.Distance(ray1.origin, transform.worldToLocalMatrix.MultiplyPoint(ray.GetPoint(distance)));
        }

        // 先计算上下两个面 
        if (PlaneUp.GetPoint(ray1, out info.point, distance))
        { 
            if (Vector3.Distance(info.point, PlaneUp.Center) <= radius)
            {
                info.point = transform.localToWorldMatrix.MultiplyPoint(info.point);
                info.normal = transform.localToWorldMatrix.MultiplyVector(PlaneUp.Normal).normalized;
                return true;
            }
        }

        if (PlaneDown.GetPoint(ray1, out info.point, distance))
        {
            if (Vector3.Distance(info.point, PlaneDown.Center) <= radius)
            {
                info.point = transform.localToWorldMatrix.MultiplyPoint(info.point);
                info.normal = transform.localToWorldMatrix.MultiplyVector(PlaneDown.Normal).normalized;
                return true;
            }
        }

        // 计算射线与圆柱的射线最近的点

        Ray cylinder = new Ray(PlaneDown.Center, Vector3.up);
        Vector3 point1, point2;

        float point1_distance, point2_distance;

        ray1.MiniDistancePoint(cylinder, out point1_distance, out point2_distance);

        point1 = ray1.GetPoint(point1_distance);
        point2 = cylinder.GetPoint(point2_distance);

        //Debug.DrawLine(transform.localToWorldMatrix.MultiplyPoint(point1), transform.localToWorldMatrix.MultiplyPoint(point2), Color.yellow);


        if (Intersects(ray1, cylinder))
        {
            // 说明两条射线相交

            float angle = Vector3.Angle(ray1.direction,cylinder.direction);

            float d = radius / Mathf.Sin(angle * Mathf.Deg2Rad);

            if (distance >= 0 && point1_distance - d > distance)
                return false;

            if (point1_distance - d < 0) 
                return false;

            info.point = ray1.GetPoint(point1_distance - d);
              
            if (info.point.y <= PlaneUp.Center.y && info.point.y >= PlaneDown.Center.y)
            {
                info.normal = transform.localToWorldMatrix.MultiplyVector(info.point - new Vector3(Center.x,info.point.y,Center.z)).normalized ;
                info.point = transform.localToWorldMatrix.MultiplyPoint(info.point);
                return true;
            } 
        }
        else
        {

            float d = Vector3.Distance(point1, point2);

            if (d <= radius)
            {
                float dis = point1_distance - Mathf.Sqrt(radius * radius - d * d);

                if (dis < 0) return false;

                if (distance >= 0 && dis > distance)
                    return false;

                info.point = ray1.GetPoint(dis);
                if (info.point.y <= PlaneUp.Center.y && info.point.y >= PlaneDown.Center.y)
                {
                    info.normal = transform.localToWorldMatrix.MultiplyVector(info.point - new Vector3(Center.x, info.point.y, Center.z)).normalized;
                    info.point = transform.localToWorldMatrix.MultiplyPoint(info.point);
                    return true;
                }
            }
        }

        return false;
    }

    public override bool Intersects(Bounds bounds)
    {   
        // 立方体的中心点在圆柱体内部
        if (Contains(bounds.center))
            return true;

        // 圆柱体的中心点在立方体内部
        if (bounds.Contains(transform.localToWorldMatrix.MultiplyPoint(Center))) 
            return true;
        

        // 上面
        plane.SetCenterAndNormal(bounds.center + Vector3.up * bounds.size.y / 2, Vector3.up, bounds.size.x, bounds.size.z, Vector3.right, Vector3.forward);

        if (CheckPlane(plane))
        {
            //Debug.Log("上面相交!");
            return true;
        }

        // 下面
        plane.SetCenterAndNormal(bounds.center + Vector3.down * bounds.size.y / 2, Vector3.down, bounds.size.x, bounds.size.z, Vector3.right, Vector3.forward);

        if (CheckPlane(plane))
        {
            //Debug.Log("下面相交!");
            return true;
        }

        // 左面
        plane.SetCenterAndNormal(bounds.center + Vector3.left * bounds.size.x / 2, Vector3.left, bounds.size.z, bounds.size.y, Vector3.forward, Vector3.up);

        if (CheckPlane(plane))
        {
            //Debug.Log("左面相交!");
            return true;
        }


        // 右面
        plane.SetCenterAndNormal(bounds.center + Vector3.right * bounds.size.x / 2, Vector3.right, bounds.size.z, bounds.size.y, Vector3.forward, Vector3.up);

        if (CheckPlane(plane))
        {
            //Debug.Log("右面相交!");
            return true;
        }


        // 前面
        plane.SetCenterAndNormal(bounds.center + Vector3.forward * bounds.size.z / 2, Vector3.forward, bounds.size.x, bounds.size.y, Vector3.right, Vector3.up);

        if (CheckPlane(plane))
        {
            //Debug.Log("前面相交!");
            return true;
        }


        // 后面
        plane.SetCenterAndNormal(bounds.center + Vector3.back * bounds.size.z / 2, Vector3.back, bounds.size.x, bounds.size.y, Vector3.right, Vector3.up);

        if (CheckPlane(plane))
        {
            //Debug.Log("后面相交!");
            return true;
        }


        return false;
    }

    public override bool Intersects(OctreeBounds octreeBounds)
    { 
        if (octreeBounds is OctreeBoxBounds)
        {
            return IntersectsWithBox(octreeBounds as OctreeBoxBounds);
        }
        else if (octreeBounds is OctreeCylinderBounds)
        {
            return IntersectsWithCylinder(octreeBounds as OctreeCylinderBounds);
        }
        else if (octreeBounds is OctreeSphereBounds) 
        {
            OctreeSphereBounds sphere = octreeBounds as OctreeSphereBounds;
            return sphere.Intersects(this);
        }
        
        return false;
    }


    private bool Intersects(Ray ray1, Ray ray2)
    { 
        // 先求出垂直这两个射线的向量
        Vector3 normal = Vector3.Cross(ray1.direction, ray2.direction); 
        // 任意在两个向量上分别取一个点 计算出向量
        Vector3 m = ray1.origin - ray2.origin;
        // 求出该向量在法线上的投影 就是两个向量之间的最小距离   
        return Mathf.Approximately(Vector3.Dot(m.normalized, normal.normalized) * Vector3.Distance(ray1.origin, ray2.origin), 0);
    }



    private bool Intersects(Ray ray, OctreePlane plane) 
    { 
        Vector3 center_direction = plane.Center - ray.origin;

        Vector3 normal = Vector3.Cross(center_direction, ray.direction);

        Vector3 direction = Vector3.Cross(normal, ray.direction).normalized;

        if(Vector3.Angle(direction,center_direction) > 90)
            direction = -direction;

        ray.origin = ray.origin + direction * radius;

        //Debug.DrawLine(transform.localToWorldMatrix.MultiplyPoint( ray.origin), transform.localToWorldMatrix.MultiplyPoint(ray.origin + ray.direction * Height), Color.white);
        // 这个距离没有问题 因为这个射线是相对的 当前矩阵中的射线 面是转换过来的
        if(plane.GetPoint(ray,out p,Height))
            return true;

        ////计算圆柱偏移
        //float offset_width = radius / Mathf.Sin(Vector3.Angle(plane.WidthDirection, ray.direction) * Mathf.Deg2Rad);
        //float offset_height = radius / Mathf.Sin(Vector3.Angle(plane.HeightDirection, ray.direction) * Mathf.Deg2Rad);
 
        //float width = plane.Width + offset_width * 2;
        //float height = plane.Height + offset_height * 2;
        //plane.SetSize(width, height);

        //float angle = 90 - (180 - Vector3.Angle(plane.Normal, ray.direction));

        //float heigth_offset = 0;

        //if (Mathf.Abs(angle - 90) > 0.01f)
        //    heigth_offset = radius / Mathf.Tan(angle * Mathf.Deg2Rad);
         
        //if (plane.GetPoint(ray, out p , Height + heigth_offset))
        //{ 
        //    Debug.DrawLine( transform.localToWorldMatrix.MultiplyPoint( ray.origin), transform.localToWorldMatrix.MultiplyPoint(p),Color.blue);

        //    // 计算是否满足
        //    return true;
        //}
         
        return false;
    }

    // 检测是否和这个面相交
    private bool CheckPlane(OctreePlane plane)
    {
        // 转换
        plane.Transform(transform.worldToLocalMatrix);
        // 判断射线是否相交
        if (Intersects(Ray, plane) || Intersects(RayInverse, plane)) {
            //Debug.Log("射线相交");
            return true;
        }

        // 判断面与面之间是否相交 
          
        if (PlaneUp.IntersectPoint(plane, out p))
        {
            //Debug.LogFormat("上面与面相交:{0}",p);

            if (PlaneUp.Contains(p) && plane.Contains(p) && Vector3.Distance(p, PlaneUp.Center) <= radius)
            {
                //Debug.Log("上面与面相交");
                return true;
            }
        }

        if (PlaneDown.IntersectPoint(plane, out p))
        {
            //Debug.LogFormat("下面与面相交:{0}", p);

            //Debug.DrawLine( transform.localToWorldMatrix.MultiplyPoint( PlaneDown.Center), transform.localToWorldMatrix.MultiplyPoint(p), Color.yellow);

            if (PlaneDown.Contains(p) && plane.Contains(p) && Vector3.Distance(p, PlaneDown.Center) <= radius)
            {
                //Debug.Log("下面与面相交");
                return true;
            }
        }


        return false;
    }

    private bool IntersectsWithBox(OctreeBoxBounds box)
    {
        if(box == null) return false;

        // 立方体的中心点在圆柱体内部
        if (Contains( box.transform.localToWorldMatrix.MultiplyPoint( box.Center)))
            return true;

        // 圆柱体的中心点在立方体内部
        if (box.Contains(transform.localToWorldMatrix.MultiplyPoint(Center)))
            return true;


        // 上面
        OctreePlane plane = box.PlaneUp;
        plane.Transform(box.transform.localToWorldMatrix);
       
        if (CheckPlane(plane))
        {
            //Debug.Log("上面相交!");
            return true;
        }


        // 下面
        plane = box.PlaneDown;
        plane.Transform(box.transform.localToWorldMatrix);
         
        if (CheckPlane(plane))
        {
            //Debug.Log("下面相交!");
            return true;
        }

        // 左面
        plane = box.PlaneLeft;
        plane.Transform(box.transform.localToWorldMatrix);

        if (CheckPlane(plane))
        {
            //Debug.Log("左面相交!");
            return true;
        }


        // 右面
        plane = box.PlaneRight;
        plane.Transform(box.transform.localToWorldMatrix);

        if (CheckPlane(plane))
        {
            //Debug.Log("右面相交!");
            return true;
        }


        // 前面
        plane = box.PlaneFront;
        plane.Transform(box.transform.localToWorldMatrix);

        if (CheckPlane(plane))
        {
            //Debug.Log("前面相交!");
            return true;
        }


        // 后面
        plane = box.PlaneBack;
        plane.Transform(box.transform.localToWorldMatrix);

        if (CheckPlane(plane))
        {
            //Debug.Log("后面相交!");
            return true;
        }
         
        return false;
    }


    private bool IntersectsWithCylinder(OctreeCylinderBounds other)
    {
        if (other == null) return false;

        if(Contains(other.transform.localToWorldMatrix.MultiplyPoint( other.Center)))
            return true;

        if (other.Contains(transform.localToWorldMatrix.MultiplyPoint(Center)))
            return true;

        //other.ray

        // 找出射线的最近点 判断点是否在圆柱内 判断距离 

        Ray ray_other = new Ray(); 
        ray_other.origin = other.transform.localToWorldMatrix.MultiplyPoint(other.Ray.origin);
        ray_other.direction = other.transform.localToWorldMatrix.MultiplyVector(other.Ray.direction).normalized;

        Vector3 other_max = other.transform.localToWorldMatrix.MultiplyPoint(other.Ray.origin + other.Ray.direction * other.Height);

        Ray ray = new Ray();
        ray.origin = transform.localToWorldMatrix.MultiplyPoint(Ray.origin);
        ray.direction = transform.localToWorldMatrix.MultiplyVector(Ray.direction);

        Vector3 max = transform.localToWorldMatrix.MultiplyPoint(Ray.origin + Ray.direction * Height);

        float ray_distance;
        float ray_other_distance;

        ray.MiniDistancePoint(ray_other, out ray_distance, out ray_other_distance);

        if (ray_distance >= 0 && ray_distance <= Vector3.Distance(max, ray.origin)
            && ray_other_distance >= 0 && ray_other_distance <= Vector3.Distance(other_max, ray_other.origin))
        {
            Vector3 point1 = ray.GetPoint(ray_distance);
            Vector3 point2 = ray_other.GetPoint(ray_other_distance);

            Vector3 direction = point1 - point2;

            Vector3 localDir = other.transform.worldToLocalMatrix.MultiplyVector(direction).normalized;
            Vector3 other_dir_point = other.transform.localToWorldMatrix.MultiplyPoint(other.Ray.origin + localDir * other.Radius);


            direction = -direction;

            localDir = transform.worldToLocalMatrix.MultiplyVector(direction).normalized;
            Vector3 dir_point = transform.localToWorldMatrix.MultiplyPoint(Ray.origin + localDir * Radius);

            if (Vector3.Distance(point1, point2) <= Vector3.Distance(other_dir_point, ray_other.origin) + Vector3.Distance(dir_point, ray.origin))
            {
                return true;
            }
        }
        else 
        {
            

            if (Intersect(this, other))
                return true;
            

            if (Intersect(other, this))
                return true;
            
        }



        return false;
    }


    public bool Intersect(OctreeCylinderBounds cylinder1, OctreeCylinderBounds cylinder2) 
    {
        Vector3 oc = cylinder2.transform.localToWorldMatrix.MultiplyPoint(cylinder2.Center);

        Vector3 other_center = cylinder1.transform.worldToLocalMatrix.MultiplyPoint(oc);

        Vector3 center_direction = other_center - cylinder1.Center;

        Vector3 normal = Vector3.Cross(center_direction, Vector3.up);

        Vector3 direction = Vector3.Cross(normal, Vector3.up).normalized;

        if (Vector3.Angle(direction, center_direction) > 90)
            direction = -direction;

        Vector3 point = cylinder1.transform.localToWorldMatrix.MultiplyPoint(cylinder1.Center - Vector3.up * cylinder1.Height / 2 + direction * cylinder1.Radius);
        
        Vector3 point_top = cylinder1.transform.localToWorldMatrix.MultiplyPoint(cylinder1.Center + Vector3.up * cylinder1.Height / 2 + direction * cylinder1.Radius);

        Ray r = new Ray(point, cylinder1.transform.up);

        //float distance = Vector3.Distance(cylinder2.transform.worldToLocalMatrix.MultiplyPoint(point), cylinder2.transform.worldToLocalMatrix.MultiplyPoint(point_top));
         
        if (cylinder2.IntersectRay(r , Vector3.Distance(point,point_top))) 
        {
            //Debug.DrawLine(r.origin, p, Color.yellow);
            return true;
        }

        return false;
    }

    public override Bounds GetBounds()
    { 
        world_bounds.center = transform.localToWorldMatrix.MultiplyPoint(Center);
        world_bounds.size = Vector3.zero;

        Vector3 top_left_down = Center + Vector3.up * Height / 2 + Vector3.left * Radius + Vector3.back * Radius; 
        Vector3 top_left_up = Center + Vector3.up * Height / 2 + Vector3.left * Radius + Vector3.forward * Radius; 
        Vector3 top_right_down = Center + Vector3.up * Height / 2 + Vector3.right * Radius + Vector3.back * Radius; 
        Vector3 top_right_up = Center + Vector3.up * Height / 2 + Vector3.right * Radius + Vector3.forward * Radius; 
        Vector3 bottom_left_down = Center + Vector3.down * Height / 2 + Vector3.left * Radius + Vector3.back * Radius; 
        Vector3 bottom_left_up = Center + Vector3.down * Height / 2 + Vector3.left * Radius + Vector3.forward * Radius; 
        Vector3 bottom_right_down = Center + Vector3.down * Height / 2 + Vector3.right * Radius + Vector3.back * Radius; 
        Vector3 bottom_right_up = Center + Vector3.down * Height / 2 + Vector3.right * Radius + Vector3.forward * Radius;

        world_bounds.Encapsulate(transform.localToWorldMatrix.MultiplyPoint(top_left_down));
        world_bounds.Encapsulate(transform.localToWorldMatrix.MultiplyPoint(top_left_up));
        world_bounds.Encapsulate(transform.localToWorldMatrix.MultiplyPoint(top_right_down));
        world_bounds.Encapsulate(transform.localToWorldMatrix.MultiplyPoint(top_right_up));
        world_bounds.Encapsulate(transform.localToWorldMatrix.MultiplyPoint(bottom_left_down));
        world_bounds.Encapsulate(transform.localToWorldMatrix.MultiplyPoint(bottom_left_up));
        world_bounds.Encapsulate(transform.localToWorldMatrix.MultiplyPoint(bottom_right_down));
        world_bounds.Encapsulate(transform.localToWorldMatrix.MultiplyPoint(bottom_right_up));

        return world_bounds;
    }

    #endregion

}
